/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
//#include <mex.h>
#include <windows.h>
#include "particleMath.h"
#include "tpose.h"


/**
* Allocates memory for Pose particle distribution   
* size: for how many particles the memory is allocated
* 
**/

TPoseParticleDistribution newPoseParticleDistribution(int size){
	TPoseParticleDistribution PPD;

	PPD.Particles = (TPoseParticle *) malloc(size*sizeof(TPoseParticle));
	PPD.size = size;
	PPD.NumOfParticles=0;
	PPD.outLiers = 0;
	PPD.Lik=0;

	return PPD;
}

/**
* frees the allocated pose distribution
*
*/
void freePoseParticleDistribution(TPoseParticleDistribution PPD){
	free(PPD.Particles);
}


/************************************************************
* PROBABILITY DISTRIBUTION FUNCTIONS 
*************************************************************/


void newRandomSeed(){
	 srand( (unsigned)time( NULL ) );
}


/**
* Returns a (pseudo) random number between 0 and 1.
**/
float uniformRandom(void)
{
  return ( ((float) rand() / (float) RAND_MAX));
}

/**********************************************************
Returns a normally distributed deviate with zero mean and unit variance, 
using unifomRandom() as the source of uniform deviates.
***************************************************************/

float normalRandom()
{
	static float gset;
	float fac,rsq,v1,v2;
	
	do {
		v1=2.0*uniformRandom()-1.0; //pick two uniform numbers in the square extending
		//from -1 to +1 in each direction, 
		v2=2.0*uniformRandom()-1.0;
		rsq=v1*v1+v2*v2;		//see if they are in the unit circle,
		//mexPrintf("Randomizing!!\n");
	} while (rsq >= 1.0 || rsq == 0.0); //if not try again
	
	fac=sqrt(-2.0*log(rsq)/rsq);
	gset=v1*fac; // can be used also as normal distributed random number!!
	//mexPrintf("DONE Randomizing!!\n");
	return v2*fac;
}

/*********************************************
* Transforms normally distributed random variable (variance 1, mean 0)
* According to the covariance cov
* xyRand: includes the normal random variable, which is returned transformed
* cov[0] = X - var
* cov[1] = XY -covar
* cov[2] = Y - var
***/  
void covRandom(float& xRand, float& yRand, float cov[3]){
	float a,b,c;
	float eig1,eig2;
	float vx1,vy1,vx2,vy2;
	float d;
	float x,y;

	// calculate eigen values

	a = 1.0;
	b = -(cov[0]+cov[2]);
	c = cov[0]*cov[2] - cov[1]*cov[1];

	eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
	eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);

	// calculate eigen vectors

	vx1 = cov[1];
	vy1 = eig1 - cov[0];
	d = sqrt(vx1*vx1+vy1+vy1);
	vx1 /= d;
	vy1 /= d;

	vx2 = cov[1];
	vy2 = eig2 - cov[0];
	d = sqrt(vx2*vx2+vy2+vy2);
	vx2 /= d;
	vy2 /= d;
	
	// Transform the random variable according to the covariance

	x = xRand * sqrt(eig1);
	y = yRand * sqrt(eig2);
	
	xRand = x * vx1 + y * vx2;
	yRand = x * vy1 + y * vy2;
}
	
TPose calculateMeanPosition(TPoseParticleDistribution PPD){
	float sumX=0,sumY=0,sumA=0;
	int i;
	TPose pos;

	for(i=0;i<PPD.NumOfParticles;i++){
		sumX += PPD.Particles[i].x;
		sumY += PPD.Particles[i].y;
		sumA += PPD.Particles[i].a;
	}
	pos.x = sumX / (float) PPD.NumOfParticles;
	pos.y = sumY / (float) PPD.NumOfParticles;
	pos.a = sumA / (float) PPD.NumOfParticles;

	pos.time=0;

	return pos;
}

TPose calculateParticleVariances(TPoseParticleDistribution PPD){
	TPose avg,var;
	int i;

	var.x=0; var.y=0; var.a=0; var.time=0;
	
	avg = calculateMeanPosition(PPD);

	for(i=0;i<PPD.NumOfParticles;i++){
		var.x += (PPD.Particles[i].x - avg.x)*(PPD.Particles[i].x - avg.x);
		var.y += (PPD.Particles[i].y - avg.y)*(PPD.Particles[i].y - avg.y);
		var.a += (PPD.Particles[i].a - avg.a)*(PPD.Particles[i].a - avg.a);
	}
	
	var.x = var.x / (PPD.NumOfParticles-1);
	var.y = var.y / (PPD.NumOfParticles-1);
	var.a = var.a / (PPD.NumOfParticles-1);

	return var;
}

/**
* Calculates initial pose particle distribution. 
* Samples from normal distribution, thus the initial position is expected to 
* be somewhat known (e.g. within 3mx3m area with angle uncertainty of 10 deg if 
* approx. 800 particles is used)
*  
* TPose pos initial position estimate
* float Q[3] uncertainty (std) of the initial position
* Q[0] x - uncertainty
* Q[1] y - uncertainty
* Q[2] angle uncertainty
* int size: the number of particles in distribution
* 
* returns normally distributed particle distribution
* OBS! creates TPoseParticleDistribution	
*/
TPoseParticleDistribution getInitialDistribution(TPose initPos,
												 float Q[3], 
												 int size
												 ){
	TPoseParticleDistribution PPD;
	int i;
	
	PPD = newPoseParticleDistribution(size);
	
	for(i=0;i<size;i++){
		PPD.Particles[i].x = initPos.x + normalRandom() * Q[0];
		PPD.Particles[i].y = initPos.y + normalRandom() * Q[1];
		PPD.Particles[i].a = initPos.a + normalRandom() * Q[2];
		PPD.Particles[i].lik = 1.0;
		PPD.Particles[i].p = 1.0 / size;
		PPD.NumOfParticles++;
	}
	return PPD;
}



TPoseParticleDistribution SIRupdate(TPoseParticleDistribution PPD){
	TPoseParticleDistribution tmp;
	float U,Q;
	int i,j,k;
	
	U = uniformRandom() / (float) PPD.NumOfParticles;
	//U = 0.1 / (float) PPD.NumOfParticles;
	j=0;
	Q=0.0;
	i=0;
	k=0;

	tmp = newPoseParticleDistribution(PPD.NumOfParticles);
	
	tmp.Lik = PPD.Lik;
	tmp.outLiers = PPD.outLiers;
	tmp.NumOfParticles = PPD.NumOfParticles;

	while(U < 1.0){
		
		if(Q>U){
			U += 1.0/PPD.NumOfParticles;
			tmp.Particles[i]=PPD.Particles[k];
			tmp.Particles[i].p = 1.0 / PPD.NumOfParticles;
			i++;
		}
		else{
			k=j;
			Q += PPD.Particles[j].p;
			j++;
		}
	}//While
	
	freePoseParticleDistribution(PPD);

	return tmp;
}


/**
* Makes the prediction step using odometry estimate and covariance matrix
* the x and y noise can be correlated, but the angle noise is independent
* 
* PPD:  Posteriori distribution - returned as priori disribution
* v[3]: odometry estimate (dx,dy,da)
* Q[4]: Uncertainty (Var(X) Var(XY) Var(Y) STD(A)!!)
**/
TPoseParticleDistribution penaPredictCov(TPoseParticleDistribution PPD, 
									  float v[3], 
									  float Q[4])
{
	int i;
	float dx,dy,dl;
	float cov[3];
	float xRand,yRand;
	float a,b,c;
	float eig1=1,eig2=1;
	float vx1,vy1,vx2,vy2;
	float d;
	float x,y;

	// calculate eigen values
	cov[0] = Q[0];
	cov[1] = Q[1];
	cov[2] = Q[2];
	
	a = 1.0;
	b = -(cov[0]+cov[2]);
	c = cov[0]*cov[2] - cov[1]*cov[1];
	
	if( (b*b-4*c)>=0){
		eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
		eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);
		
		// calculate eigen vectors
		if(eig1 != eig2){
			vx1 = cov[1];
			vy1 = eig1 - cov[0];
			d = sqrt(vx1*vx1+vy1*vy1);
			vx1 /= d;
			vy1 /= d;
			
			vx2 = cov[1];
			vy2 = eig2 - cov[0];
			d = sqrt(vx2*vx2+vy2*vy2);
			vx2 /= d;
			vy2 /= d;
		}
		else{
				vx1 = 1.0;
				vx2 = 0.0;
				vy1 = 0.0;
				vy2 = 1.0;
		}
		
	}
	else{
		fprintf(stderr,"IMAGINARY EIGEN VALUES\n");
		eig1=1;
		eig2=1;
		
		vx1 = 1.0;
		vx2 = 0.0;
		vy1 = 0.0;
		vy2 = 1.0;
	}
	
	
	// Transform the random variable according to the covariance
	
	dl = sqrt(v[0]*v[0]+v[1]*v[1]);
	
	for(i=0;i<PPD.NumOfParticles;i++){
		xRand = normalRandom();
		yRand = normalRandom();
		
		x = xRand * sqrt(eig1);
		y = yRand * sqrt(eig2);
		
		xRand = x * vx1 + y * vx2;
		yRand = x * vy1 + y * vy2;
		//mexPrintf("XR: %.3lf, YR:%.3lf x:%.3lf y%.3lf eig1:%.3lf eig2:%.3lf, %.3lf %.3lf %.3lf %.3lf\n",xRand,yRand,x,y,eig1,eig2,vx1,vy1,vx2,vy2);
		//Sleep(50);
		//covRandom(xRand,yRand,cov);

		dx = dl * cos(PPD.Particles[i].a + v[2]);
		dy = dl * sin(PPD.Particles[i].a + v[2]);

		PPD.Particles[i].x = PPD.Particles[i].x + dx + xRand;
		PPD.Particles[i].y = PPD.Particles[i].y + dy + yRand;
		PPD.Particles[i].a = PPD.Particles[i].a + v[2] + normalRandom()* Q[3];
	}
return PPD;
}


/**
* Makes the prediction step using odometry estimate and uncertainty, 
* the uncertainties (noise) are expected to be independed 
* 
* PPD:  Posteriori distribution - returned as priori disribution
* v[3]: odometry estimate (dx,dy,da)
* Q[3]: Uncertainty (STD(X) STD(Y) STD(A))
**/
TPoseParticleDistribution penaPredict(TPoseParticleDistribution PPD, 
									  float v[3], 
									  float Q[3])
{
#define DIST_TO_PROB	1.0 / 0.2 //20cm means 1 x Noise
#define ANG_TO_PROB		1.0 / (5.0 * PI / 180.0)
	int i;
	float dx,dy,dl;
	float offset =0.0f ; // -PI/2.0f;
	float a;
	float QQ[3];

	dl = sqrt(v[0]*v[0]+v[1]*v[1]);
	a  = atan2(v[1],v[0]);

	if(a<0) a = 2.0f * PI + a;
	
	//fprintf(stderr,"A:%.2f dl:%.2f\n",a,dl);
/*
	if(dl<0.01){
		Q[0] = 0.01f;
		Q[1] = 0.01f;
	}
	else if(dl<0.05){
		Q[0] = 0.02f;
		Q[1] = 0.02f;
	}
	else if(dl<0.1){
		Q[0] = 0.05f;
		Q[1] = 0.05f;
	}
*/	
	QQ[0] = Q[0];
	QQ[1] = Q[1];
	QQ[2] = Q[2];

	if(dl<0.01){
		QQ[0] = Q[0]*0.1f; //0.01f;
		QQ[1] = Q[0]*0.1f;
		QQ[2] = Q[2]*0.1f;
	}
	else if(dl<0.05){
		QQ[0] = Q[0]*0.2f; //0.02f;
		QQ[1] = Q[0]*0.2f;
	}
	else if(dl<0.1){
		QQ[0] = Q[0]*0.5f;// 0.05f;
		QQ[1] = Q[0]*0.5f;
	}

	for(i=0;i<PPD.NumOfParticles;i++){
		
		dx = dl * cos(PPD.Particles[i].a + offset + a);
		dy = dl * sin(PPD.Particles[i].a + offset + a);

		PPD.Particles[i].x = PPD.Particles[i].x + dx + normalRandom()* QQ[0];
		PPD.Particles[i].y = PPD.Particles[i].y + dy + normalRandom()* QQ[1];
		PPD.Particles[i].a = PPD.Particles[i].a + v[2] + normalRandom()* QQ[2];
	}
return PPD;
}


TPoseParticleDistribution normalizeDistribution(TPoseParticleDistribution PPD){
	int i;
	float summ=0;

	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p *= PPD.Particles[i].lik; 
		summ+=PPD.Particles[i].p;
	}
	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p = PPD.Particles[i].p/summ;
	}
	return PPD;
}



TPoseParticleDistribution normalizeASIRDistribution(
													TPoseParticleDistribution PPD,
													TPoseParticleDistribution PPD2)
{
	int i;
	float summ=0;

	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p *= PPD.Particles[i].lik / PPD2.Particles[i].lik; 
		summ+=PPD.Particles[i].p;
	}
	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p = PPD.Particles[i].p/summ;
	}
	return PPD;
}




/**********************************************************************
* getScanLines
* Returns line parameters (a,b,c) ax+by+c=0 for one scan going through 
* Position Pos
*
***************************************************/ 
void getScanLines(float *scanLines,TPoseParticle P, int number_of_measurements){
	int i;
	float alpha;
	float phii;

#ifdef USE_MCL
	phii=P.a; //-PI/2.0f;
#endif
	phii=P.a;//NOTE THIS YARI -PI/2.0;

	//mexPrintf("PHII: %lf\n",phii);
	if(phii>2.0*PI) phii-= (float)(2.0*PI);
	if(phii<-2.0*PI) phii+=(float)(2.0*PI);
	
	for(i=0;i<number_of_measurements;i++){
		alpha=phii+(180.0 / number_of_measurements) * (PI/180.0)*i ;

		if( (fabs( fabs( alpha ) - PI/2.0 )) <0.01 || (fabs( fabs( alpha ) - 3.0*PI/2.0 )) <0.01  ){
			scanLines[3*i]=1;
			scanLines[3*i+1]=0;
			scanLines[3*i+2]=-P.x;
		}
		else{
			scanLines[3*i]=-tan(alpha);
			scanLines[3*i+1]=1;
			scanLines[3*i+2]=tan(alpha)*P.x-P.y;
		}
	}
}


/************************************************
* Calculates virtual scan from line map
* returns *Scan (x, y) in robot coordinate system
* Function is required for likehood calculations
**************************************************/
int findVirtualScan(float *Scan, 
					 float *scanLines, 
					 TLineVector map,
					 TPoseParticle Pos,
					 int number_of_measurements){
	int i;
	int j;
	float a1,b1,c1,a2,b2,c2; //Line parameters
	float x0,y0,x,y,px,py,xr,yr,Ret_xr=100,Ret_yr=100;
	float mx1,my1,mx2,my2,L,AC,AB;
	float DIST,distTmp;
	float S,C;
	int isIt;
	int outLiers=0;

#ifdef USE_MCL
	S=sin(Pos.a);//NOTE THIS YARI-PI/2);
	C=cos(Pos.a);//NOTE THIS YARI-PI/2);
	//S=sin(Pos.a-PI/2.0);//NOTE THIS YARI-PI/2);
	//C=cos(Pos.a-PI/2.0);//NOTE THIS YARI-PI/2);
#else
	S=sin(Pos.a);//NOTE THIS YARI-PI/2);
	C=cos(Pos.a);//NOTE THIS YARI-PI/2);
#endif

	for(i=0;i<number_of_measurements;i++){
		a1=scanLines[3*i];
		b1=scanLines[3*i+1];
		c1=scanLines[3*i+2];
		DIST=LASER_MAX_DIST2; //Note map distances are in CM's !!
		x=1000;
		y=1000;
		Ret_xr=1000;
		Ret_yr=1000;
		for(j=0;j<map.NumOfLines;j++){
			isIt=1;
			a2=map.Lines[j].p.a; // Lines[j*3];
			b2=map.Lines[j].p.b; // Lines[j*3+1];
			c2=map.Lines[j].p.c; // Lines[j*3+2];

			if( fabs(a1*b2-a2*b1)<0.000001 ){
				//mexPrintf("Parallel\n");
				isIt=0; //Lines are parallel
			}
			else
			{
				//Calculation of intersection
				x0 = (-c1*b2 + c2*b1) / (a1*b2-a2*b1);
				y0 = (-c2*a1 + a2*c1) / (a1*b2-a2*b1);
				
				px=(x0-Pos.x);
				py=(y0-Pos.y);
				
				xr=C * px + S * py;
				yr=-S * px + C * py; //xr,yr in robot coordinate system
				
				if(yr>=0) isIt=1; 
				else{
					//mexPrintf("COORDINATES\n");
					isIt=0; //The intersection was not in right direction
				}
			}// ELSE
			
			if(isIt){
				mx2=map.Lines[j].x1; //map[j*N];
				my2=map.Lines[j].y1; //map[j*N+1];
				mx1=map.Lines[j].x2; //map[j*N+2];
				my1=map.Lines[j].y2; //map[j*N+3];

				L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
				AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
				AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
				if(AC<=L && AB<=L ) isIt=1;
				else{
					//mexPrintf("NOT INSIDE\n");
					isIt=0; // not inside the map line segment
				}
			}
			distTmp = ( px*px + py*py );

			if( distTmp <= DIST && isIt==1){
				x=x0;
				y=y0;
				DIST=distTmp; //closest hit so far
				Ret_xr = xr; // In "robot" coordinate system
				Ret_yr = yr; // In "robot" coordinate system
			}
			//else mexPrintf("DIST TAI JOKU AIEMPI\n");

		}//FOR J
		//Scan[2*i]=x;  // FOR ABSOLUTE COORDINATES
		//Scan[2*i+1]=y;
		
		Scan[2*i]=Ret_xr;   // FOR RELATIVE COORDINATES
		Scan[2*i+1]=Ret_yr;
		
		if(Ret_xr==1000)outLiers++;

		//mexPrintf("Xr:%lf Yr:%lf\n",Ret_xr,Ret_yr);
	}//for I

return outLiers;
}


/************************************************
* Find the lines that are visible from pose Pos
* Map is the line map, Pos is the 
**************************************************/
TLineVector findVisibilityLines( 
					 TLineVector map,
					 TPoseParticle Pos
					 ){
	int i;
	int j,k;
	float a1,b1,c1,a2,b2,c2; //Line parameters
	float x0,y0,x,y,px,py,xr=0,yr=0,Ret_xr=100,Ret_yr=100;
	float mx1,my1,mx2,my2,L,AC,AB;
	float DIST,distTmp;
	float S,C;
	int isIt;
	int outLiers=0;
	float *scanLines;
	int NUM_OF_SCANS = 180;
	TLineVector LV;
	TLine Line;
	int *lineIndex;
	int tmpIndex;

	scanLines = (float *) malloc(3 * NUM_OF_SCANS * sizeof(float));
	lineIndex = (int *) malloc(NUM_OF_SCANS * sizeof(int));

	LV = newLineVector(NUM_OF_SCANS);
	
	getScanLines(scanLines,Pos, NUMBER_OF_MEASUREMENTS);
	
	S=sin(Pos.a-PI/2);
	C=cos(Pos.a-PI/2);

	for(i=0;i<NUM_OF_SCANS;i++){
		a1=scanLines[3*i];
		b1=scanLines[3*i+1];
		c1=scanLines[3*i+2];
		DIST=LASER_MAX_DIST2; //Note map distances are in CM's !!
		x=1000;
		y=1000;
		Ret_xr=1000;
		Ret_yr=1000;
		tmpIndex = -1;
		for(j=0;j<map.NumOfLines;j++){
			isIt=1;
			a2=map.Lines[j].p.a; // Lines[j*3];
			b2=map.Lines[j].p.b; // Lines[j*3+1];
			c2=map.Lines[j].p.c; // Lines[j*3+2];

			if( fabs(a1*b2-a2*b1)<0.000001 ){
				//mexPrintf("Parallel\n");
				isIt=0; //Lines are parallel
			}
			else
			{
				//Calculation of intersection
				x0 = (-c1*b2 + c2*b1) / (a1*b2-a2*b1);
				y0 = (-c2*a1 + a2*c1) / (a1*b2-a2*b1);
				
				px=(x0-Pos.x);
				py=(y0-Pos.y);
				
				//xr=C * px + S * py;
				yr=-S * px + C * py; //xr,yr in robot coordinate system
				
				if(yr>=0) isIt=1; 
				else{
					//mexPrintf("COORDINATES\n");
					isIt=0; //The intersection was not in right direction
				}
			}// ELSE
			
			if(isIt){
				mx2=map.Lines[j].x1; //map[j*N];
				my2=map.Lines[j].y1; //map[j*N+1];
				mx1=map.Lines[j].x2; //map[j*N+2];
				my1=map.Lines[j].y2; //map[j*N+3];

				L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
				AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
				AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
				if(AC<=L && AB<=L ) isIt=1;
				else{
					//mexPrintf("NOT INSIDE\n");
					isIt=0; // not inside the map line segment
				}
			}
			distTmp = ( px*px + py*py );

			if( distTmp <= DIST && isIt==1){
				x=x0;
				y=y0;
				DIST=distTmp; //closest hit so far
				Ret_xr = xr; // In "robot" coordinate system
				Ret_yr = yr; // In "robot" coordinate system
				Line = map.Lines[j];
				tmpIndex = j;

			}
			//else mexPrintf("DIST TAI JOKU AIEMPI\n");

		}//FOR J
		
		if(tmpIndex>=0){ // some line found
			int isNewLine = 1;
			
			for(k=0;k<LV.NumOfLines;k++){ // go through allready marked lines
				if(lineIndex[k]==tmpIndex) isNewLine = 0; //The line was allready in vector
			}

			if(isNewLine){ 
				LV.Lines[LV.NumOfLines] = Line;
				LV.NumOfLines++;
			}
		}

		//mexPrintf("Xr:%lf Yr:%lf\n",Ret_xr,Ret_yr);
	}//for I

	free(lineIndex);
	free(scanLines);

return LV;
}






/*****************************************************
* Calculates the likelihood of particle distribution
* PPD: The particle distribution
* Map: Line map
* Scan: Current measurement in robot coordinate system (x,y)
*************************************************************/

TPoseParticleDistribution calculateLikelihood(
											  TPoseParticleDistribution PPD, 
											  TLineVector Map,
											  float *Scan
											  )
{
	float *scanLines, *virtualScan;
	int i,j;
	float dx,dy,dL2;
	float Lik;
	float Liks;
	int outLiers;

	scanLines = (float *) malloc(3 * NUMBER_OF_MEASUREMENTS * sizeof(float));
	virtualScan = (float *) malloc (2 * NUMBER_OF_MEASUREMENTS * sizeof(float));
	PPD.Lik=0;
	for(i=0;i<PPD.NumOfParticles;i++){
		
		getScanLines(scanLines,PPD.Particles[i], NUMBER_OF_MEASUREMENTS);
		outLiers=findVirtualScan(virtualScan,scanLines,Map,
			PPD.Particles[i],NUMBER_OF_MEASUREMENTS);	
		
		PPD.outLiers+=(float) outLiers;
		
		Liks = 1;
		
		for(j=0;j<NUMBER_OF_MEASUREMENTS;j++){
			dx = Scan[2*j] - virtualScan[2*j];
			dy = Scan[2*j+1] - virtualScan[2*j+1];
			dL2 = dx*dx + dy*dy; 
			
			if( dy< -0.15) {
				Lik = exp(  - (dL2/(VMEAS)) ) + 1.5*FALSE_PROB;
				Liks = Liks*Lik;
			}
			else{
				Lik = exp(  - (dL2/(VMEAS)) ) + FALSE_PROB;
				Liks = Liks*Lik;
			}
		}
		
		PPD.Lik+=Liks;
		PPD.Particles[i].lik=Liks;
	}
	PPD.outLiers = (PPD.outLiers / ( NUMBER_OF_MEASUREMENTS * PPD.NumOfParticles))*100;
	//debug(DEBUGS,"LIK: %.2f\n",PPD.Lik);

	free(scanLines);
	free(virtualScan);

	return PPD;
}

/**
* Estimates initial position using ASIR.
*
* TPose initPos is the estimate of the initial position
* int size, is the size of the particle distribution used for estimation
* float v[3] is the odometry estimate for ASIR step
* Q[3] is the uncertainty of the initial distribution	  
* TLineVector cMap is the line map
* float *scan is the actual measurement taken from initPose + v[3]
*
* Returns estimated pose distribution
*/ 
TPoseParticleDistribution estimateInitialPosition( 
												  TPose initPose,
												  int size,
												  float v[3],
									   			  float Q[3],
									   			  TLineVector cMap,
												  float *scan)
{
	TPoseParticleDistribution PPD;
	float Qp[3]; // "Prediction error
	float Qp0[3]={0,0,0};



	Qp[0] = (float)0.2; // x-variance
	Qp[1] = (float)0.2; // y-variance
	Qp[2] = (float)(2.0 * PI / 180.0); // angle STD!!!

	PPD = getInitialDistribution(initPose,Q,size);
	//PPD = ASIRStep(PPD,v,Qp0,cMap,scan);
	//PPD = ASIRStep(PPD,v,Qp,cMap,scan);

	return PPD;
}

TPoseParticleDistribution MCLBasicStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan)
{
	PPD = penaPredict(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeDistribution(PPD);
	//mexPrintf("SIR...\n");

	PPD = SIRupdate(PPD);

return PPD;
}

TPoseParticleDistribution MCLBasicStepNoSIR(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan)
{
	PPD = penaPredict(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeDistribution(PPD);
	//mexPrintf("SIR...\n");

return PPD;
}

TPoseParticleDistribution MCLBasicCovStep(
										  TPoseParticleDistribution PPD,
										  float v[3],
										  float Q[4],
										  TLineVector cMap,
										  float *scan)
{
	PPD = penaPredictCov(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeDistribution(PPD);
	//mexPrintf("SIR...\n");

	PPD = SIRupdate(PPD);

return PPD;
}

TPoseParticleDistribution ASIRStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan)
{	
	TPoseParticleDistribution tmp;
	int i;

	tmp = newPoseParticleDistribution(PPD.NumOfParticles);
	
	for(i=0;i<PPD.NumOfParticles;i++){
		tmp.Particles[i] = PPD.Particles[i];
		tmp.NumOfParticles++;
		
	}
	
	//mexPrintf("nPPD: %d, nTmp:%d\n",PPD.NumOfParticles,tmp.NumOfParticles);
	//for(i=0;i<3;i++)
	//	mexPrintf("v[%d]:%f Q[%d]:%f\n",i,v[i],i,Q[i]);

	//mexPrintf("Predict...\n");
	tmp = penaPredict(tmp,v,Q);
	//mexPrintf("Likelihood...\n");

	tmp = calculateLikelihood(tmp,cMap,scan);
	//mexPrintf("Normalize...\n");

	tmp = normalizeDistribution(tmp);
	
	
	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p = tmp.Particles[i].p;
	}
	//mexPrintf("SIR...\n");
	PPD = SIRupdate(PPD);
	
	PPD = penaPredict(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeASIRDistribution(PPD,tmp);
	//mexPrintf("SIR...\n");

	PPD = SIRupdate(PPD);

	freePoseParticleDistribution(tmp);
return PPD;
}